/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_INTERFACE_H_
#define CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_INTERFACE_H_

#include <functional>
#include <memory>
#include <string>

#include "absl/memory/memory.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer/mapping/submaps.h"
#include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/landmark_data.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/sensor/timed_point_cloud_data.h"

namespace cartographer
{
    namespace mapping
    {
        proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(common::LuaParameterDictionary *const parameter_dictionary);

        class LocalSlamResultData;

        // This interface is used for both 2D and 3D SLAM. Implementations wire up a
        // global SLAM stack, i.e. local SLAM for initial pose estimates, scan matching
        // to detect loop closure, and a sparse pose graph optimization to compute
        // optimized pose estimates.
        class TrajectoryBuilderInterface
        {
        public:
            struct InsertionResult
            {
                mapping::NodeId node_id;
                std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
                std::vector<std::shared_ptr<const mapping::Submap>> insertion_submaps;
                std::vector<int> dynamicCnts;
            };

            // A callback which is called after local SLAM processes an accumulated
            // 'sensor::RangeData'. If the data was inserted into a submap, reports the
            // assigned 'NodeId', otherwise 'nullptr' if the data was filtered out.
            using LocalSlamResultCallback =
                std::function<void(int /* trajectory ID */, common::Time, transform::Rigid3d /* local pose estimate */,
                                   sensor::RangeData /* in local frame */, std::unique_ptr<const InsertionResult>)>;

            struct SensorId
            {
                enum class SensorType
                {
                    RANGE = 0,
                    IMU,
                    ODOMETRY,
                    FIXED_FRAME_POSE,
                    LANDMARK,
                    LOCAL_SLAM_RESULT
                };

                SensorType type;
                std::string id;

                bool operator==(const SensorId &other) const
                {
                    return std::forward_as_tuple(type, id) == std::forward_as_tuple(other.type, other.id);
                }

                bool operator<(const SensorId &other) const
                {
                    return std::forward_as_tuple(type, id) < std::forward_as_tuple(other.type, other.id);
                }
            };

            TrajectoryBuilderInterface()
            {
            }
            virtual ~TrajectoryBuilderInterface()
            {
            }

            TrajectoryBuilderInterface(const TrajectoryBuilderInterface &) = delete;
            TrajectoryBuilderInterface &operator=(const TrajectoryBuilderInterface &) = delete;

            virtual void AddSensorData(const std::string &sensor_id, const sensor::TimedPointCloudData &timed_point_cloud_data) = 0;
            virtual void AddSensorData(const std::string &sensor_id, const sensor::ImuData &imu_data) = 0;
            virtual void AddSensorData(const std::string &sensor_id, const sensor::OdometryData &odometry_data) = 0;
            virtual void AddSensorData(const std::string &sensor_id, const sensor::FixedFramePoseData &fixed_frame_pose) = 0;
            virtual void AddSensorData(const std::string &sensor_id, const sensor::LandmarkData &landmark_data) = 0;
            // Allows to directly add local SLAM results to the 'PoseGraph'. Note that it
            // is invalid to add local SLAM results for a trajectory that has a
            // 'LocalTrajectoryBuilder2D/3D'.
            virtual void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData> local_slam_result_data) = 0;
        };

        proto::SensorId ToProto(const TrajectoryBuilderInterface::SensorId &sensor_id);
        TrajectoryBuilderInterface::SensorId FromProto(const proto::SensorId &sensor_id_proto);

    } // namespace mapping
} // namespace cartographer

#endif // CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_INTERFACE_H_
